/*
 * File: out_leg_position.c
 *
 * MATLAB Coder version            : 5.3
 * C/C++ source code generated on  : 11-Dec-2023 21:53:46
 */

/* Include Files */
#include "../include/matlab_code/leg_position.h"
#include <math.h>

/* Function Definitions */
/*
 * OUT_LEG_POSITION
 *     POS = OUT_LEG_POSITION(PHI1,PHI4)
 *
 * Arguments    : float phi1
 *                float phi4
 *                float pos[2]
 * Return Type  : void
 */
void leg_position(float phi1, float phi4, float pos[2])
{
  float xc,yc,xb,yb,xd,yd;
  float A0,B0,C0;
  float phi2;
  /*     This function was generated by the Symbolic Math Toolbox version 9.2.
   */
  /*     27-Jan-2023 19:36:58 */
  xb = 0.05f*cosf(phi1);
  yb = 0.05f*sinf(phi1);
  xd = 0.06f+0.05f*cosf(phi4);
  yd = 0.05f*sinf(phi4);

  // A0 = 2*0.105f*(xd-xb);
  // B0 = 2*0.105f*(yd-yb);
  A0 = 0.21f*(xd-xb);
  B0 = 0.21f*(yd-yb); //简化运算
  C0 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);

  phi2 = 2*atanf((B0+sqrtf(A0*A0+B0*B0-C0*C0))/(A0+C0));

  xc = xb + 0.105f*cosf(phi2);
  yc = yb + 0.105f*sinf(phi2);


  // pos[0] = sqrtf((xc-0.06f/2)*(xc-0.06f/2)+yc*yc);
  // pos[1] = atan2f(yc,(xc-0.06f/2));
  pos[0] = sqrtf((xc-0.03f)*(xc-0.03f)+yc*yc);
  pos[1] = atan2f(yc,(xc-0.03f)); //简化运算
}


/*
 * File trailer for out_leg_position.c
 *
 * [EOF]
 */
